// -*- C++ -*-
/*!
 * @file  UsbCameraMonitor.cpp
 * @brief Sequence InPort component
 *
 */
#include "UsbCameraMonitor.h"

// Module specification
// <rtc-template block="module_spec">
static const char* col_spec[] =
  {
    "implementation_id", "UsbCameraMonitor",
    "type_name",         "UsbCameraMonitor",
    "description",       "UsbCameraMonitor component",
    "version",           "1.0",
    "vendor",            "Tomoaki Yoshikai, JSK",
    "category",          "example",
    "activity_type",     "DataFlowComponent",
    "max_instance",      "10",
    "language",          "C++",
    "lang_type",         "compile",
    "conf.default.print_name",         "0",
    ""
  };
// </rtc-template>

UsbCameraMonitor::UsbCameraMonitor(RTC::Manager* manager)
  : RTC::DataFlowComponentBase(manager),
    // <rtc-template block="initializer">
    m_ImageDataIn( "ImageData",m_ImageData )
    // </rtc-template>
{
}

UsbCameraMonitor::~UsbCameraMonitor()
{
}

RTC::ReturnCode_t UsbCameraMonitor::onInitialize()
{

  // Registration: InPort/OutPort/Service
  // <rtc-template block="registration">
  // Set InPort buffers
  addInPort( "ImageData", m_ImageDataIn );    
    
  // Set service provider to Ports

  // Set CORBA Service Ports

  m_Width = 320;
  m_Height = 240;

  windowName = std::string(getInstanceName());  
  // </rtc-template>
  if (m_file.empty())
    bindParameter("file", m_file, "UsbCameraMonitorTest.conf");
  bindParameter("config", m_config, "UsbCameraMonitor0");
  bindParameter( "WaitTime", m_WaitTime, "10" );

  return RTC::RTC_OK;
}


/*
RTC::ReturnCode_t UsbCameraMonitor::onFinalize()
{
  return RTC::RTC_OK;
}
*/

/*
RTC::ReturnCode_t UsbCameraMonitor::onStartup(RTC::UniqueId ec_id)
{
  return RTC::RTC_OK;
}
*/

/*
RTC::ReturnCode_t UsbCameraMonitor::onShutdown(RTC::UniqueId ec_id)
{
  return RTC::RTC_OK;
}
*/

RTC::ReturnCode_t UsbCameraMonitor::onActivated(RTC::UniqueId ec_id)
{
  //ウィンドウを作成する
  std::cerr << windowName <<std::endl;
  cvNamedWindow( windowName.c_str(), CV_WINDOW_AUTOSIZE );
  image = cvCreateImage( cvSize( m_Width, m_Height ), IPL_DEPTH_8U, 3 );
  cvSet( image, cvScalarAll( 0 ), NULL );
  cvShowImage( windowName.c_str(), image );
  cvWaitKey( m_WaitTime );
   
  fprintf(stderr, "Finish initialization\n");
  return RTC::RTC_OK;
}


RTC::ReturnCode_t UsbCameraMonitor::onDeactivated(RTC::UniqueId ec_id)
{
  cvDestroyWindow( windowName.c_str() );

  return RTC::RTC_OK;
}


RTC::ReturnCode_t UsbCameraMonitor::onExecute(RTC::UniqueId ec_id)
{
  try
  {
    if ( !m_ImageDataIn.isNew() )
      {
        return RTC::RTC_OK;
      }
    m_ImageDataIn.read();

    //recreate image
    if(m_ImageData.width != m_Width || m_ImageData.height != m_Height){
      m_Width = m_ImageData.width;
      m_Height = m_ImageData.height;
      image = cvCreateImage( cvSize( m_Width, m_Height ), IPL_DEPTH_8U, 3 );
    }

    //receive image
    int nLength;
    nLength = m_ImageData.pixels.length();
    memcpy(image->imageData,(void *)&(m_ImageData.pixels[0]), m_ImageData.pixels.length());


    cvShowImage( windowName.c_str(), image );
    cvWaitKey( m_WaitTime );
  }
  catch(const std::runtime_error &e)
  {
      // 失敗しても続行
      std::cerr << e.what() << std::endl;
  }

  return RTC::RTC_OK;
}

/*
RTC::ReturnCode_t UsbCameraMonitor::onAborting(RTC::UniqueId ec_id)
{
  return RTC::RTC_OK;
}
*/

/*
RTC::ReturnCode_t UsbCameraMonitor::onError(RTC::UniqueId ec_id)
{
  return RTC::RTC_OK;
}
*/

/*
RTC::ReturnCode_t UsbCameraMonitor::onReset(RTC::UniqueId ec_id)
{
  return RTC::RTC_OK;
}
*/

/*
RTC::ReturnCode_t UsbCameraMonitor::onStateUpdate(RTC::UniqueId ec_id)
{
  return RTC::RTC_OK;
}
*/

/*
RTC::ReturnCode_t UsbCameraMonitor::onRateChanged(RTC::UniqueId ec_id)
{
  return RTC::RTC_OK;
}
*/

extern "C"
{
 
  void UsbCameraMonitorInit(RTC::Manager* manager)
  {
    coil::Properties profile(col_spec);
    manager->registerFactory(profile,
                             RTC::Create<UsbCameraMonitor>,
                             RTC::Delete<UsbCameraMonitor>);
  }
  
};


